
#ifndef __MLS_SKELMESHDEFORMER_H__
#define __MLS_SKELMESHDEFORMER_H__

#include "../core/tconstraints.h"
#include "mlsMeshDeformer.h"
#include "gskeleton.h"

template <class T>
class MlsSkelMeshDeformer :public MlsMeshDeformer<T> {
   
   typedef TConstraints<Point3>     PConstraints;
   typedef TConstraints<Segment3>   SConstraints;
public:
   virtual ~MlsSkelMeshDeformer() {}
   
   MlsSkelMeshDeformer(): skeleton(0) {
      this->coSource = 0;
      this->coTarget = 0;
      this->model = 0;
   }
   
   /// Constructor
   /// @param s the skeleton
   /// @param m the mesh model
   MlsSkelMeshDeformer(GSkeleton *s, Mesh *m) {
      // copy the initial positions
      assert(m != 0);
      assert(s != 0);
      skeleton = s;
      this->model = m;
      this->ipositions.resize(this->model->nvertices());
      this->inormals.resize(this->model->nvertices());
      Vertex_iterator vi = this->model->vertices_begin();
      for (unsigned int i=0; i<this->ipositions.size(); ++i, ++vi) {
         this->ipositions[i] = vi->point();
         this->inormals[i] = vi->normal();
      }
      
      setConstraints(skeleton, &this->cosourcE);
      
      this->isGeodesic = true;
      this->coSource = &this->cosourcE;
      this->coTarget = &cotarget;
      
      // 
      genDistanceTable(this->coSource);
      // generate the table of weights
      this->genWeightTable(this->coSource);
   }
   
   /// Constructor
   /// @param s the skeleton
   /// @param m the mesh model
   MlsSkelMeshDeformer(GSkeleton *s, Mesh *m, T &constraints) {
      // copy the initial positions
      assert(m != 0);
      assert(s != 0);
      skeleton = s;
      this->model = m;
      this->ipositions.resize(this->model->nvertices());
      this->inormals.resize(this->model->nvertices());
      Vertex_iterator vi = this->model->vertices_begin();
      for (unsigned int i=0; i<this->ipositions.size(); ++i, ++vi) {
         this->ipositions[i] = vi->point();
         this->inormals[i] = vi->normal();
      }
      
      this->cosourcE = constraints; // the initial constraints
      this->isGeodesic = false;
      this->coSource = &this->cosourcE;
      this->coTarget = &cotarget;
      
      // 
      genDistanceTable(this->coSource);
      // generate the table of weights
      this->genWeightTable(this->coSource);
   }
   
   /// Executes a deformation using constraints defined by a set of constraints
   /// @param constraints the deformed constraints
   virtual void execute(T &constraints) {
      
      //~ cotarget = constraints; // the deformed constraints
      MlsMeshDeformer<T>::execute(constraints);
   }
   
   /// Executes a deformation using constraints defined by a new skeleton
   /// @param skeltarget the new skeleton
   virtual void execute(GSkeleton *skeltarget) {
      
      setConstraints(skeltarget, &cotarget);
      MlsMeshDeformer<T>::execute(cotarget);
   }
   
protected:
   
   virtual void setConstraints(GSkeleton *skel, PConstraints *constraints) {
      cout<<"setConstraints:POINTS"<<endl;
      for (int i=0; i<skel->nJoints(); ++i) {
         constraints->push_back(skel->joints[i]);
      }
   }
   
   virtual void setConstraints(GSkeleton *skel, SConstraints *constraints) {
      cout<<"setConstraints:SEGMENTS"<<endl;
      for (int i=0; i<skel->nBones(); ++i) {
         constraints->push_back(skel->getBoneSegment(i));
      }
   }
   
   virtual void genDistanceTable(const PConstraints *constraints) {
      
      cout<<" ... computing the distance table (PConstraints) mlsskelmeshdeformer... "<<endl;
      const int &njoints = skeleton->nJoints();
      this->distanceTable.resize(this->model->nvertices());
      for (uint i=0; i<this->distanceTable.size(); ++i) {
         this->distanceTable[i].resize(njoints,0.0);
      }
      
      bool isPossible = false; /// its possible to compute a geodesic metric over the structure?
      if (this->model->isClustered) isPossible = true;
      
      if (isPossible and this->isGeodesic) {
         cout<<" ... computing vertex/joint distances walking over the skeleton... "<<endl;
         uint ii=0; Number t=0;
         Vertex_iterator vi = this->model->vertices_begin();
         while (vi != this->model->vertices_end()) {
            const Point3 &p = vi->point();
            assert(vi->closestBone != -1);
            const int &idbone = vi->closestBone; /// the closest joint
            const pair<int, int> &bone = skeleton->bones[idbone];
            const Point3 &A = skeleton->getJointPos(bone.first);
            const Point3 &B = skeleton->getJointPos(bone.second);
            const Number dbone = skeleton->getDistance(p, idbone, t);
            
            const Point3 c = B + t*(A-B); // the closest point over the bone
            const Vector3 cA(c, A);
            const Vector3 cB(c, B);
            const Number dist[2] = {sqrt(cA*cA), sqrt(cB*cB)};
            
            vector<Number> dA(njoints,0.0);
            vector<Number> dB(njoints,0.0);
            skeleton->floodfill_Dijkstra(bone.first);
            for (int j=0; j<njoints; ++j) dA[j] = skeleton->d[j];
            skeleton->floodfill_Dijkstra(bone.second);
            for (int j=0; j<njoints; ++j) dB[j] = skeleton->d[j];
            
            for (int j=0; j<njoints; ++j) {
               const Number djoint = (dA[j]<dB[j])?dist[0]:dist[1]; // distance to the closest joint
               this->distanceTable[ii][j] = dbone + djoint + min(dA[j],dB[j]);
            }
            ++vi; ++ii;
         }
      } else {
         cout<<" ... computing euclidian distances ... "<<endl;
         for (uint i=0; i<this->model->nvertices(); ++i) {
            for (uint j=0; j<this->coSource->size(); ++j) {
               const Point3 &P = this->ipositions[i];
               const Point3 &p = constraints->getConstraint(j);
               Vector3 v(p,P);
               this->distanceTable[i][j] = sqrt(v*v);
            }
         }
      }
   }
   
   virtual void genDistanceTable(const SConstraints *constraints) {
      
      cout<<" ... computing the distance table (dt: for segments mlsskelmeshdeformer) ... "<<endl;
      this->dtTable.resize(this->model->nvertices());
      for (uint i=0; i<this->dtTable.size(); ++i) {
         this->dtTable[i].resize(constraints->size(),make_pair(0.0,0.0));
      }
      
      bool isPossible = false; /// its possible to compute a geodesic metric over the structure?
      if (this->model->isClustered) isPossible = true;
      
      if (isPossible and this->isGeodesic) {
         
         cout<<" ... computing vertex/constraints(segment) distances walking over the skeleton... "<<endl;
         const int &njoints = skeleton->nJoints();
         uint i=0; Number t;
         Vertex_iterator vi = this->model->vertices_begin();
         while (vi != this->model->vertices_end()) {
            const Point3 &p = vi->point();
            assert(vi->closestBone != -1);
            const int &idbone = vi->closestBone; /// the closest bone
            const pair<int, int> &bone = skeleton->bones[idbone];
            const Point3 &A = skeleton->getJointPos(bone.first);
            const Point3 &B = skeleton->getJointPos(bone.second);
            const Number dbone = skeleton->getDistance(p, idbone, t);
            
            //~ cout<<p<<" :: "<<boneSegment<<" :: "<<dbone<<" : "<<t<<endl;
            
            const Point3 c = B + t*(A-B); // the closest point over the bone
            const Vector3 cA(c, A);
            const Vector3 cB(c, B);
            const Number dist[2] = {sqrt(cA*cA), sqrt(cB*cB)};
            
            vector<Number> dA(njoints,0.0);
            vector<Number> dB(njoints,0.0);
            skeleton->floodfill_Dijkstra(bone.first);
            for (int j=0; j<njoints; ++j) dA[j] = skeleton->d[j];
            skeleton->floodfill_Dijkstra(bone.second);
            for (int j=0; j<njoints; ++j) dB[j] = skeleton->d[j];
            
            for (int j=0; j<skeleton->nBones(); ++j) {
               if (j==idbone) {
                  this->dtTable[i][j] = make_pair(dbone,t);
               } else {
                  const pair<int, int> &bi = skeleton->bones[j];
                  int joboA = (dA[bi.first]<dA[bi.second])? bi.first:bi.second;
                  int joboB = (dB[bi.first]<dB[bi.second])? bi.first:bi.second;
                  assert(joboA == joboB);
                  const int &jobo = joboA;
                  int jobi = (dA[jobo]<dB[jobo])? bone.first:bone.second;
                  Number dista = (dA[jobi]<dB[jobi])? dA[jobi]:dB[jobi];
                  Number tt = (jobi == bi.first)? 1.0:0.0;
                  
                  Number djoint = (dA[jobi]<dB[jobi])? dist[0]:dist[1]; // distance to the closest joint
                  Number d = dbone + djoint + dista;
                  this->dtTable[i][j] = make_pair(d,tt);
               }
            }
            ++vi; ++i;
         }
         
      } else {
         cout<<" ... computing euclidian distances vertex/constraints(segment) ... "<<endl;
         Number t;
         for (uint i=0; i<this->model->nvertices(); ++i) {
            const Point3 &P = this->ipositions[i];
            for (uint j=0; j<constraints->size(); ++j) {
               const Segment3 &segment = constraints->getConstraint(j);
               Number d = computeDistanceMinSegment(P,segment,t);
               this->dtTable[i][j] = make_pair(d,t);
            }
         }
      }
      
      //~ cout<<"the distance table ..."<<endl;
      //~ for (uint i=0; i<this->dtTable.size(); ++i) {
         //~ cout<<i<<": ";
         //~ for (uint j=0; j<this->dtTable[i].size(); ++j) {
            //~ cout<<this->dtTable[i][j].first<<" "<<this->dtTable[i][j].second<<" : ";
         //~ }
         //~ cout<<endl;
      //~ }
   }
   
protected:
   //~ T                          cosource; // constraints source
   T                          cotarget; // constraints target
   GSkeleton                  *skeleton; // the source skeleton
};

#endif
